/*
 * AdvancedCDNET Example
 * Demonstrates all enhanced CDNET features including:
 * - Multi-network support
 * - Multicast messaging
 * - Socket abstraction
 * - Layer 0 and Layer 1 protocols
 * - Advanced addressing
 */

#include <SimpleCDNET.h>

// Create CDNET instance
SimpleCDNET cdnet;

// Socket definitions
cdn_sock_t server_sock;
cdn_sock_t client_sock;
cdn_sock_t multicast_sock;

// Network configuration
const uint8_t NETWORK_ID = 0x01;    // Network identifier
const uint8_t MAC_ADDRESS = 0x42;   // This device's MAC address
const uint8_t SERVER_PORT = 1234;   // Server port
const uint8_t CLIENT_PORT = 5678;   // Client port
const uint8_t MULTICAST_PORT = 8888; // Multicast port

// Message buffers
uint8_t tx_buffer[64];
uint8_t rx_buffer[64];

void setup() {
    Serial.begin(115200);
    while (!Serial);
    
    Serial.println("=== Advanced CDNET Example ===");
    
    // Initialize CDNET with network ID and MAC address
    cdnet.begin(&Serial1, NETWORK_ID, MAC_ADDRESS);
    cdnet.setBaudRate(115200);
    
    // Enable advanced features
    cdnet.enableMulticast(true);
    cdnet.enableMultiNet(true);
    
    // Setup server socket
    server_sock.port = SERVER_PORT;
    server_sock.ns = cdnet.getNamespace();
    server_sock.tx_only = false;
    
    // Setup client socket
    client_sock.port = CLIENT_PORT;
    client_sock.ns = cdnet.getNamespace();
    client_sock.tx_only = false;
    
    // Setup multicast socket
    multicast_sock.port = MULTICAST_PORT;
    multicast_sock.ns = cdnet.getNamespace();
    multicast_sock.tx_only = false;
    
    // Bind sockets
    if (cdnet.bindSocket(&server_sock, SERVER_PORT) == 0) {
        Serial.println("Server socket bound to port " + String(SERVER_PORT));
    }
    
    if (cdnet.bindSocket(&client_sock, CLIENT_PORT) == 0) {
        Serial.println("Client socket bound to port " + String(CLIENT_PORT));
    }
    
    if (cdnet.bindSocket(&multicast_sock, MULTICAST_PORT) == 0) {
        Serial.println("Multicast socket bound to port " + String(MULTICAST_PORT));
    }
    
    Serial.println("Setup complete. Ready for communication.");
    Serial.println("MAC Address: 0x" + String(MAC_ADDRESS, HEX));
    Serial.println("Network ID: 0x" + String(NETWORK_ID, HEX));
    Serial.println();
}

void loop() {
    cdnet.update();  // Process incoming packets
    
    // Handle Layer 0 messages
    handleLayer0Messages();
    
    // Handle Layer 1 messages
    handleLayer1Messages();
    
    // Handle multicast messages
    handleMulticastMessages();
    
    // Handle socket-based communication
    handleSocketCommunication();
    
    // Send periodic messages
    static unsigned long last_message = 0;
    if (millis() - last_message > 2000) {
        sendPeriodicMessages();
        last_message = millis();
    }
    
    delay(100);
}

void handleLayer0Messages() {
    uint8_t src_addr, port, len;
    
    len = cdnet.readL0(&src_addr, &port, rx_buffer, sizeof(rx_buffer));
    if (len > 0) {
        Serial.print("[L0] Received from 0x");
        Serial.print(src_addr, HEX);
        Serial.print(" port ");
        Serial.print(port);
        Serial.print(": ");
        
        for (uint8_t i = 0; i < len; i++) {
            Serial.print("0x");
            Serial.print(rx_buffer[i], HEX);
            Serial.print(" ");
        }
        Serial.println();
        
        // Echo response
        cdnet.sendL0(src_addr, port, rx_buffer, len);
    }
}

void handleLayer1Messages() {
    uint8_t src_addr;
    uint16_t port;
    uint8_t len;
    
    len = cdnet.readL1(&src_addr, &port, rx_buffer, sizeof(rx_buffer));
    if (len > 0) {
        Serial.print("[L1] Received from 0x");
        Serial.print(src_addr, HEX);
        Serial.print(" port ");
        Serial.print(port);
        Serial.print(": ");
        
        for (uint8_t i = 0; i < len; i++) {
            Serial.print("0x");
            Serial.print(rx_buffer[i], HEX);
            Serial.print(" ");
        }
        Serial.println();
        
        // Process Layer 1 message
        processLayer1Message(src_addr, port, rx_buffer, len);
    }
}

void handleMulticastMessages() {
    cdn_pkt_t *pkt = cdnet.receivePacket(&multicast_sock);
    if (pkt) {
        Serial.print("[Multicast] Received from ");
        Serial.print(pkt->src.addr[0], HEX);
        Serial.print(":");
        Serial.print(pkt->src.addr[1], HEX);
        Serial.print(":");
        Serial.print(pkt->src.addr[2], HEX);
        Serial.print(" port ");
        Serial.print(pkt->src.port);
        Serial.print(" len ");
        Serial.println(pkt->len);
        
        // Process multicast packet
        processMulticastPacket(pkt);
        
        // Free the packet
        cdn_pkt_free(cdnet.getNamespace(), pkt);
    }
}

void handleSocketCommunication() {
    // Handle server socket
    cdn_pkt_t *server_pkt = cdnet.receivePacket(&server_sock);
    if (server_pkt) {
        Serial.print("[Server] Client request from ");
        Serial.print(server_pkt->src.addr[0], HEX);
        Serial.print(":");
        Serial.print(server_pkt->src.addr[1], HEX);
        Serial.print(":");
        Serial.print(server_pkt->src.addr[2], HEX);
        Serial.print(" port ");
        Serial.println(server_pkt->src.port);
        
        // Process server request
        processServerRequest(server_pkt);
        
        cdn_pkt_free(cdnet.getNamespace(), server_pkt);
    }
    
    // Handle client socket
    cdn_pkt_t *client_pkt = cdnet.receivePacket(&client_sock);
    if (client_pkt) {
        Serial.print("[Client] Response from ");
        Serial.print(client_pkt->src.addr[0], HEX);
        Serial.print(":");
        Serial.print(client_pkt->src.addr[1], HEX);
        Serial.print(":");
        Serial.print(client_pkt->src.addr[2], HEX);
        Serial.print(" port ");
        Serial.println(client_pkt->src.port);
        
        processClientResponse(client_pkt);
        cdn_pkt_free(cdnet.getNamespace(), client_pkt);
    }
}

void sendPeriodicMessages() {
    static uint8_t counter = 0;
    
    // Send Layer 0 broadcast
    sprintf((char*)tx_buffer, "Hello L0 #%d", counter);
    cdnet.sendL0(0xFF, 0x10, tx_buffer, strlen((char*)tx_buffer));
    
    // Send Layer 1 broadcast
    sprintf((char*)tx_buffer, "Hello L1 #%d", counter);
    cdnet.sendL1(0xFF, 0x20, tx_buffer, strlen((char*)tx_buffer));
    
    // Send multicast message
    cdn_pkt_t *pkt = cdn_pkt_get(cdnet.getNamespace());
    if (pkt) {
        pkt->dst.addr[0] = 0xF0;  // Multicast address
        pkt->dst.addr[1] = 0x01;  // Network group
        pkt->dst.addr[2] = 0xFF;  // All nodes
        pkt->dst.port = MULTICAST_PORT;
        
        sprintf((char*)pkt->dat, "Multicast #%d", counter);
        pkt->len = strlen((char*)pkt->dat);
        
        cdnet.sendPacket(&multicast_sock, pkt);
    }
    
    counter++;
}

void processLayer1Message(uint8_t src, uint16_t port, uint8_t *data, uint8_t len) {
    Serial.print("Processing L1 message on port ");
    Serial.println(port);
    
    // Example: Echo back with modified data
    if (len > 0) {
        data[0] = ~data[0];  // Invert first byte
        cdnet.sendL1(src, port, data, len);
    }
}

void processMulticastPacket(cdn_pkt_t *pkt) {
    Serial.print("Multicast data: ");
    for (uint8_t i = 0; i < pkt->len && i < 32; i++) {
        Serial.print(pkt->dat[i]);
    }
    Serial.println();
}

void processServerRequest(cdn_pkt_t *pkt) {
    // Simple echo server
    cdn_pkt_t *response = cdn_pkt_get(cdnet.getNamespace());
    if (response) {
        response->dst = pkt->src;  // Send back to sender
        response->src = pkt->dst;  // From server
        
        sprintf((char*)response->dat, "Echo: %s", pkt->dat);
        response->len = strlen((char*)response->dat);
        
        cdnet.sendPacket(&server_sock, response);
    }
}

void processClientResponse(cdn_pkt_t *pkt) {
    Serial.print("Client received: ");
    for (uint8_t i = 0; i < pkt->len && i < 32; i++) {
        Serial.print(pkt->dat[i]);
    }
    Serial.println();
}